/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.DriverStationLCD.Line;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class FRC2012 extends IterativeRobot {

    //Command autonomousCommand;

    boolean prevButtonState = false;
    boolean ranAutonomous;

    private DriverStationLCD display;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        // This method ONLY happens when the robot is first turned on
        
        // instantiate the command used for the autonomous period
        //autonomousCommand = new Autonomous();

        display = DriverStationLCD.getInstance();
        display.updateLCD();
        display.println(Line.kMain6, 1, "Initializing...                      ");
        display.println(Line.kUser2, 1, "                                     ");
        display.println(Line.kUser3, 1, "                                     ");
        display.println(Line.kUser4, 1, "                                     ");
        display.println(Line.kUser5, 1, "                                     ");
        display.println(Line.kUser6, 1, "                                     ");
        display.updateLCD();

        NetworkTable.initialize();

        CommandBase.init();
        SmartDashboard.putData("SchedulerData", Scheduler.getInstance());
    }

    public void autonomousInit() {
        // schedule the autonomous command (example)
        //autonomousCommand.start();
        display.println(Line.kMain6, 1, "Initializing Autonomous...           ");
        display.println(Line.kUser2, 1, "                                     ");
        display.println(Line.kUser3, 1, "                                     ");
        display.println(Line.kUser4, 1, "                                     ");
        display.println(Line.kUser5, 1, "                                     ");
        display.println(Line.kUser6, 1, "                                     ");
        display.updateLCD();

        ranAutonomous = false;
    }


    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        if(ranAutonomous == false) {
            display.println(Line.kMain6, 1, "Running Autonomous...                ");
            display.updateLCD();

            display.println(Line.kMain6, 1, "Unlocking bridge arm lock...         ");
            display.updateLCD();
            CommandBase.bridgearm.bridgeArmUnlock();
            Timer.delay(0.5);

            display.println(Line.kMain6, 1, "Lowering bridge arm...               ");
            display.updateLCD();
            CommandBase.bridgearm.lowerArm();
            Timer.delay(1);
            CommandBase.bridgearm.bridgeArmStop();
            Timer.delay(0.01);

            display.println(Line.kMain6, 1, "Locking bridge arm...                ");
            display.updateLCD();
            CommandBase.bridgearm.bridgeArmLock();
            Timer.delay(0.5);

            display.println(Line.kMain6, 1, "Shooting balls...                    ");
            display.updateLCD();
            CommandBase.pickup.pickupBall();
            CommandBase.shooter.spinWheels(true);
            Timer.delay(10);
            
            ranAutonomous = true;
        }

        Scheduler.getInstance().run();
    }

    public void teleopInit() {
	// This makes sure that the autonomous stops running when
        // teleop starts running. If you want the autonomous to 
        // continue until interrupted by another command, remove
        // this line or comment it out.
        //autonomousCommand.cancel();

        display.println(Line.kMain6, 1, "Initializing Teleop...               ");
        display.println(Line.kUser2, 1, "                                     ");
        display.println(Line.kUser3, 1, "                                     ");
        display.println(Line.kUser4, 1, "                                     ");
        display.println(Line.kUser5, 1, "                                     ");
        display.println(Line.kUser6, 1, "                                     ");
        display.updateLCD();
        
        updateStatus();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopContinous() {
        Scheduler.getInstance().run();

        display.println(Line.kMain6, 1, "Running Teleop...                    ");
        display.updateLCD();

        CommandBase.drivetrain.toDashboardEncoders();
        CommandBase.drivetrain.updateStatus();
        updateStatus();

        if(OI.getInstance().getDriveJoystick().getRawButton(2)) {
            double[] array = CommandBase.drivetrain.findCameraDistance();
            double offset = array[1];

            CommandBase.drivetrain.proportional(0, offset);
        }

        if(!prevButtonState) {
            if(OI.getInstance().getShooterJoystick().getRawButton(8)) {
                prevButtonState = true;
                CommandBase.shooter.shooterState = !CommandBase.shooter.shooterState;
            } else {
                prevButtonState = false;
            }
        } else {
            if(!OI.getInstance().getShooterJoystick().getRawButton(8)) {
                prevButtonState = false;
            } else {
                prevButtonState = true;
            }
        }

        System.out.println(CommandBase.drivetrain.EitherOne.get());
    }

    public void disabledPeriodic() {

        display.println(Line.kMain6, 1, "Disabled                             ");
        display.println(Line.kUser2, 1, "                                     ");
        display.println(Line.kUser3, 1, "                                     ");
        display.println(Line.kUser4, 1, "                                     ");
        display.println(Line.kUser5, 1, "                                     ");
        display.println(Line.kUser6, 1, "                                     ");
        display.updateLCD();

        updateStatus();
    }
    
    public void updateStatus() {
        // Add our data to the "SmartDashboard"
        // Report the values of our joysticks
        SmartDashboard.putDouble("Drive Joystick X", OI.getInstance().getDriveJoystick().getX());
        SmartDashboard.putDouble("Drive Joystick Y", OI.getInstance().getDriveJoystick().getY());
        SmartDashboard.putDouble("Drive Joystick Twist", OI.getInstance().getDriveJoystick().getTwist());
        SmartDashboard.putDouble("Shooter Joystick X", OI.getInstance().getShooterJoystick().getX());
        SmartDashboard.putDouble("Shooter Joystick Y", OI.getInstance().getShooterJoystick().getY());
    }
}
